/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    additionTypes.h
  * @author  Mentos Seetoo
  * @brief   This file provides additional types and functions to make sure that 
  *          some dynamic tools and algorithms will be migrated successfully. 
  * @note    THe following third-party libraries are used:
  *          - STL
  *          - Eigen3	
  * @warning 
  *     - At least C++11 is required. 														 
  ******************************************************************************
  * @attention
  * 
  * if you had modified this file, please make sure there is no any bug in your
  * code. Update the version Number and write dowm your name and the date.What 
  * most important is that make sure the users will have a clear understanding
  * through your description.
  ******************************************************************************
  */
#ifndef ADDITION_TYPES_H
#define ADDITION_TYPES_H
/* Includes ------------------------------------------------------------------*/
#include <vector>
#include <eigen3/Eigen/Dense>

/* Types define --------------------------------------------------------------*/
/**
 * @brief Common types
 */
// N-Dimension Vectors
template <typename T>
using Vec2 = typename Eigen::Matrix<T, 2, 1>;

template <typename T>
using Vec3 = typename Eigen::Matrix<T, 3, 1>;

template <typename T>
using Vec4 = typename Eigen::Matrix<T, 4, 1>;

template <typename T>
using Quat = typename Eigen::Matrix<T, 4, 1>;

template <typename T>
using Vec6 = Eigen::Matrix<T, 6, 1>;

template <typename T>
using Vec10 = Eigen::Matrix<T, 10, 1>;

template <typename T>
using Vec12 = Eigen::Matrix<T, 12, 1>;

template <typename T>
using Vec18 = Eigen::Matrix<T, 18, 1>;

template <typename T>
using Vec28 = Eigen::Matrix<T, 28, 1>;

// N-Dimension Square Matrix
template <typename T>
using Mat3 = typename Eigen::Matrix<T, 3, 3>;

template <typename T>
using Mat4 = typename Eigen::Matrix<T, 4, 4>;

template <typename T>
using Mat6 = typename Eigen::Matrix<T, 6, 6>;

template <typename T>
using Mat12 = typename Eigen::Matrix<T, 12, 12>;

template <typename T>
using Mat18 = Eigen::Matrix<T, 18, 18>;

template <typename T>
using Mat28 = Eigen::Matrix<T, 28, 28>;

//!< 3-Dimension Rotation matrix
template <typename T>
using RotMat = typename Eigen::Matrix<T, 3, 3>;

//!< Spatial Vector (6x1, all subspaces)
template <typename T>
using SVec = typename Eigen::Matrix<T, 6, 1>;

//!< Spatial Transform (6x6)
template <typename T>
using SXform = typename Eigen::Matrix<T, 6, 6>;

// 10x1 Vector
template <typename T>
using MassProperties = typename Eigen::Matrix<T, 10, 1>;

// Dynamically sized vector
template <typename T>
using DVec = typename Eigen::Matrix<T, Eigen::Dynamic, 1>;

// Dynamically sized matrix
template <typename T>
using DMat = typename Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

// Dynamically sized matrix with spatial vector columns
template <typename T>
using D6Mat = typename Eigen::Matrix<T, 6, Eigen::Dynamic>;

// Dynamically sized matrix with cartesian vector columns
template <typename T>
using D3Mat = typename Eigen::Matrix<T, 3, Eigen::Dynamic>;

// std::vector (a list) of Eigen things
template <typename T>
using vectorAligned = typename std::vector<T, Eigen::aligned_allocator<T>>;
/* Utility functions ---------------------------------------------------------*/
/*!
 * Are two eigen matrices almost equal?
 */
template <typename T, typename T2>
bool almostEqual(const Eigen::MatrixBase<T>& a, const Eigen::MatrixBase<T>& b,
                 T2 tol) {
  long x = T::RowsAtCompileTime;
  long y = T::ColsAtCompileTime;

  if (T::RowsAtCompileTime == Eigen::Dynamic ||
      T::ColsAtCompileTime == Eigen::Dynamic) {
    assert(a.rows() == b.rows());
    assert(a.cols() == b.cols());
    x = a.rows();
    y = a.cols();
  }

  for (long i = 0; i < x; i++) {
    for (long j = 0; j < y; j++) {
      T2 error = std::abs(a(i, j) - b(i, j));
      if (error >= tol) return false;
    }
  }
  return true;
}

/*!
 * Not check input parameters.Make sure min < max 
 */
template <typename T>
T LimitRange(T val, T min, T max)
{
    return val >= max ? max : (val <= min ? min : val); 
}

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/